package com.guazi.trackingtest;

import android.hardware.SensorEvent;

import com.guazi.trackingtest.filter.Kalman;
import com.guazi.trackingtest.filter.Noise;

import java.util.ArrayList;
import java.util.List;

import static com.guazi.trackingtest.Utils.TYPE_LINEAR_ACC;
import static com.guazi.trackingtest.Utils.sensorEvent2PackData;

/**
 * Created by lqy on 2018/3/21.
 */
public class Filter {
    private float[] quietError = {0.0f, 0.0f, 0.0f};
    private boolean findError = false;
    private long time = 0;
    private int count = 0;
    private QuietListener mQuietListener;

    private List<PackData> filterEvents;

    private int filterSize = 10;
    private int filterPosition = -1;
    private float defaultQuiet = 0.01f;
    private Sender mSender;

    private Noise mNoise = new Noise();
    private Kalman mKalman = new Kalman();

    public Filter(int filterSize, Sender sender) {
        this.filterSize = filterSize;
        this.mSender = sender;
        filterEvents = new ArrayList<>(filterSize);
    }

    private void strategy_1(SensorEvent event) {
        PackData packData = filterEvents.get(0);
        for (int i = 1; i < filterEvents.size(); i++) {
            packData.x += filterEvents.get(i).x;
            packData.y += filterEvents.get(i).y;
            packData.z += filterEvents.get(i).z;
            if (i == filterSize - 1) {
                packData.timestamp = filterEvents.get(i).timestamp;
            }
        }
        packData.x = packData.x / filterSize + quietError[0];
        packData.y = packData.y / filterSize + quietError[1];
        packData.z = packData.z / filterSize + quietError[2];


        if (mSender != null) {
            mSender.addPackData(mKalman.filter(mNoise.filter(packData)));
//                    mSender.addPackData(mNoise.filter(packData));
        }

        filterEvents.clear();
        filterEvents.add(sensorEvent2PackData(event, TYPE_LINEAR_ACC));
    }

    private PackData temp = new PackData();

    private void strategy_2(SensorEvent event) {
        temp.setType(TYPE_LINEAR_ACC);
        temp.setX(0.0);
        temp.setY(0.0);
        temp.setZ(0.0);
        for (int i = 0; i < filterEvents.size(); i++) {
            temp.x += filterEvents.get(i).x;
            temp.y += filterEvents.get(i).y;
            temp.z += filterEvents.get(i).z;
            if (i == filterSize - 1) {
                temp.timestamp = filterEvents.get(i).timestamp;
            }
        }
        temp.x = temp.x / filterSize + quietError[0];
        temp.y = temp.y / filterSize + quietError[1];
        temp.z = temp.z / filterSize + quietError[2];

        if (mSender != null) {
//            temp = mKalman.filter(mNoise.filter(temp));
//            Log.e("lqy", "xxxxx--->" + temp.x);
//            mSender.addPackData(temp);
            mSender.addPackData(mKalman.filter(mNoise.filter(temp)));
        }

        filterEvents.remove(filterPosition);
        filterEvents.add(filterPosition, sensorEvent2PackData(event, TYPE_LINEAR_ACC));
    }

    public void addSensorEvent(SensorEvent event) {
        if (findError) {
            if (filterEvents.size() < filterSize) {
                filterEvents.add(sensorEvent2PackData(event, TYPE_LINEAR_ACC));
            } else {
                if (filterPosition == -1) {
                    filterPosition = 0;
                }
                strategy_1(event);
//                strategy_2(event);

                filterPosition++;
                if (filterPosition == filterSize) {
                    filterPosition = 0;
                }
            }
        } else {
            if (time == 0)
                time = System.currentTimeMillis();

            //静止3秒检测
            if (System.currentTimeMillis() - time > 3000) {
                quietError[0] /= count;
                quietError[1] /= count;
                quietError[2] /= count;
                findError = true;
                mNoise.setNoiseDeltaError(defaultQuiet);
                if (mQuietListener != null) {
                    mQuietListener.finishQuiet(defaultQuiet);
                }
            } else {
                if (Math.abs(event.values[0]) < defaultQuiet && Math.abs(event.values[1]) < defaultQuiet && Math.abs(event.values[2]) < defaultQuiet) {
                    quietError[0] += event.values[0];
                    quietError[1] += event.values[1];
                    quietError[2] += event.values[2];
                    count++;
                } else {
                    defaultQuiet *= 2f;
                    time = 0;
                    count = 0;
                    quietError[0] = 0.0f;
                    quietError[1] = 0.0f;
                    quietError[2] = 0.0f;
                    if (mQuietListener != null) {
                        mQuietListener.beingQuiet();
                    }
                }
            }
        }
    }

    public void reQuiet() {
        time = 0;
        count = 0;
        quietError[0] = 0.0f;
        quietError[1] = 0.0f;
        quietError[2] = 0.0f;
        findError = false;
        defaultQuiet = 0.01f;
    }

    public void setQuietListener(QuietListener listener) {
        mQuietListener = listener;
    }

    public static interface QuietListener {
        void beingQuiet();

        void finishQuiet(float defaultQuiet);
    }
}
